package image;

import java.awt.Point;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.PriorityQueue;

import javax.media.opengl.GL2;

/**Provides mechanisms for raycasting and pathfinding in a grid of open or blocked rectangles.*/
public class Grid
	{
	public static final boolean BLOCKED = true;
	public static final boolean OPEN = false;
	
	private static final byte[] xNeighbours4 = {1,0,-1,0};
	private static final byte[] yNeighbours4 = {0,-1,0,1};
	private static final byte[] neighbourDistance4 = {32,32,32,32};
	private static final byte[] xNeighbours8 = { 1, 1, 0,-1,-1,-1, 0, 1};
	private static final byte[] yNeighbours8 = { 0,-1,-1,-1, 0, 1, 1, 1};
	public static final byte[] neighbourDistance8 = {32, 45, 32, 45, 32, 45, 32, 45};
	
	public V3F pos;
	public float xScale, yScale;
	
	public final boolean[][] grid;
	/**Construct a new Grid with the given 2D dimensions(xLength, yLength).
	 * The Grid will be scaled into world space by applying the scale factors.*/
	public Grid(V3F pos, float xScale, float yScale, int xLength, int yLength)
		{
		this.pos = pos;
		
		this.xScale = xScale;
		this.yScale = yScale;
		
		this.grid = new boolean[yLength][xLength];
		}
	/**Returns whether the grid is {@link Grid#BLOCKED} or {@link Grid#OPEN} at the given position in grid space.*/
	public boolean getSquare(int x, int y)
		{
		if(y<0 || y>=grid.length || x<0 || x>=grid[0].length)
			return BLOCKED;
		else
			return grid[y][x];
		}
	/**Sets the indicated square in grid space to {@link Grid#BLOCKED} or {@link Grid#OPEN}.*/
	public void setSquare(int x, int y, boolean element)
		{
		grid[y][x] = element;
		}

	/**Converts from grid space to world space.*/
	public V3F getWorldLocation(int x, int y)
		{
		return pos.clone().add(x*xScale, y*yScale, 0);
		}
	/**Returns whether the grid is {@link Grid#BLOCKED} or {@link Grid#OPEN} at the given position in world space.*/
	public boolean getGridSquare(double x, double y)
		{
		return getSquare(getGridX(x), getGridY(y));
		}
	/**Converts from world space into grid space, and back into world space. 
	 * The result is a world location which is snapped to the nearest upper-left corner of a grid square, rounding down.*/
	public V3F getSnappedLocation(double x, double y)
		{
		return getWorldLocation(getGridX(x), getGridY(y));
		}
	/**Converts from world space into grid space.*/
	public int getGridX(double x)
		{
		return ((int)Math.floor((x-pos.x())/xScale));
		}
	/**Converts from world space into grid space.*/
	public int getGridY(double y)
		{
		return ((int)Math.floor((y-pos.y())/yScale));
		}
	/**Converts from world space into grid space.*/
	public Point getGridPoint(V3F worldPosition)
		{
		return new Point(getGridX(worldPosition.x()), getGridY(worldPosition.y()));
		}
	/**Returns the x position in world space from the indicated position in grid space.*/
	public double getWorldX(int x)
		{
		return pos.x()+xScale*x;
		}
	/**Returns the y position in world space from the indicated position in grid space.*/
	public double getWorldY(int y)
		{
		return pos.y()+yScale*y;
		}
	/**Returns the z position in world space from the indicated position in grid space.*/
	public double getWorldZ(int x, int y)
		{
		return pos.z();
		}
	/**Use raycasting from the base position in the indicated radian direction.
	 * @param base The position of the ray source, in world space.
	 * @param angle The angle of the ray, in radians, counter-clockwise, with 0 as east.
	 * @return The location, in world space, where the ray hits a blocked area or the edge of the grid.
	 */
	public V3F rayCast(V3F base, float angle)
		{
		final float h = (float)Math.cos(angle);
		final float v = -(float)Math.sin(angle);
		
		final byte hDir = (byte) Math.signum(h);
		final byte vDir = (byte) Math.signum(v);
		final float xS = h>=0?0:1;
		final float yS = v>=0?0:1;
		
		float fx, fy;
		int dx, dy;
		
		fx = (float)(base.x()-pos.x())/xScale;
		fy = (float)(base.y()-pos.y())/yScale;
		dx = (int)Math.floor(fx);
		dy = (int)Math.floor(fy);
		fx -= dx;
		fy -= dy;
		
		//It is not possible to cast any longer than this.
		int maxIterations = grid.length+grid[0].length;
		int iterations = 0;
		
		while(iterations < maxIterations)
			{
			if(getSquare(dx, dy) == BLOCKED)//If the current location is blocked, return it
				break;
			
			final float hDist;
			if(hDir == 0)//Will never reach the next block on the horizontal axis
				hDist = Float.MAX_VALUE;
			else if(hDir > 0)//Must go in the positive direction
				hDist = (1-fx)/h;
			else//Must go in the negative direction
				hDist = -fx/h;
			
			final float vDist;
			if(vDir == 0)//Will never reach the next block on the vertical axis
				vDist = Float.MAX_VALUE;
			else if(vDir > 0)//Must go in the positive direction
				vDist = (1-fy)/v;
			else//Must go in the negative direction
				vDist = -fy/v;
			
			if(hDist == vDist)//Will reach the next block simultaneously on both axes
				{
				dx += hDir;
				dy += vDir;
				fx = xS;
				fy = yS;
				}
			if(hDist < vDist) //Will reach the next horizontal block first 
				{
				dx += hDir;
				fx = xS;
				fy += hDist*v;//Note: hDist is the distance that we must have traveled to get to fx = xS on the x axis.
				}
			else//Will reach the next vertical block first;
				{
				dy += vDir;
				fy = yS;
				fx += vDist*h;//Note: vDist is the distance that we must have traveled to get to fy = yS on the y axis.
				}
			iterations++;
			}
		V3F r = pos.clone().add((dx+fx)*xScale, (dy+fy)*yScale, 0).z(base.z());
		return r;
		}
	/**
	 * Implementation of the A* algorithm.
	 * @param start The starting position on the grid. If outside, no path can be found and null will be returned.
	 * @param goal The target position on the grid. If outside, no path can be found and null will be returned.
	 * @param diagonals Whether to permit diagonal motion.
	 * @param maxUnstuckDistance The maximum amount of move on the first step, 
	 * in grid coordinates, in order to break free from being stuck in a wall.
	 * @param blockedRadius The amount of extra space that must be free on both sides, 
	 * in grid coordinates, for a position to count as open.
	 * @return Some optimal sequence of points from start to goal on the grid, or null if no path could be found.
	 */
	public Point[] findPath(Point start, Point goal, boolean diagonals, int maxUnstuckDistance, int blockedRadius)
		{
		if(getSquare(goal.x, goal.y) == BLOCKED)
			return null;
		
		byte[] xNeighbours = diagonals?xNeighbours8:xNeighbours4;
		byte[] yNeighbours = diagonals?yNeighbours8:yNeighbours4;
		byte[] neighbourDistance = diagonals?neighbourDistance8:neighbourDistance4;
		
		// The set of nodes already evaluated.
	    final HashSet<Point> closedset = new HashSet<Point>();
	    
	    //Cost from start along best known path.
	    final HashMap<Point, Integer> g_score = new HashMap<Point, Integer>();
	    g_score.put(start, 0);
	    
	    //Estimated total cost from start to goal through y.
	    final HashMap<Point, Integer> f_score = new HashMap<Point, Integer>();
	    f_score.put(start, 0 + heuristic_cost_estimate(start, goal));
	 
	    
	    // The set of tentative nodes to be evaluated, initially containing the start node
	    final PriorityQueue<Point> openset = new PriorityQueue<Point>(128, new Comparator<Point>(){
			@Override
			public int compare(Point boy, Point girl)
				{
				//Queue will always return the candidate node with the lowest estimated cost
				return f_score.get(boy)-f_score.get(girl);
				}});
	    openset.add(start);
	    
	    //The map of navigated nodes.
	    HashMap<Point, Point> came_from = new HashMap<Point, Point>();
	    
	    int maxD = maxUnstuckDistance;
	    
	    while (!openset.isEmpty())
	    	{
	    	Point current = openset.poll();
	        if (current.equals(goal))
	        	{
	        	ArrayList<Point> path = new ArrayList<Point>();
	        	reconstruct_path(came_from, goal, path);
	        	Point[] r = new Point[path.size()];
	        	path.toArray(r);
	            return r;
	        	}
	        closedset.add(current);
	        
	        addingCandidates:
	        for(int i = 0; i<xNeighbours.length; i++)
	        	{
	        	Point candidate = null;
	        	boolean blocked = false;
	        	
	        	largestDist:
	        	for(int dist = 1; dist<=maxD; dist++) {
	        		blocked = false;
		        	candidate = new Point(current);
		        	candidate.translate(xNeighbours[i]*dist, yNeighbours[i]*dist);
		        	
		        	for(int y = -blockedRadius; y<=blockedRadius; y++)
		        		for(int x = -blockedRadius; x<=blockedRadius; x++)
		        			if( Calc.sqDist(x, y) < blockedRadius*blockedRadius*1.5f && 
		        				getSquare(candidate.x+x, candidate.y+y) == BLOCKED)
		        				blocked = true;
		        	if(!blocked)
		        		break largestDist;
	        	}
	        	if(blocked)
	        		continue addingCandidates;
	        	if(closedset.contains(candidate))
	        		continue addingCandidates;
	        	
	        	int tentativeGScore = g_score.get(current) + neighbourDistance[i];
	        	
	        	if(!openset.contains(candidate) || tentativeGScore < g_score.get(candidate))
	        		{
	        		came_from.put(candidate, current);
	        		g_score.put(candidate, tentativeGScore);
	        		f_score.put(candidate, tentativeGScore+heuristic_cost_estimate(candidate, goal));
	        		if(!openset.contains(candidate))
	        			openset.add(candidate);
	        		}
	        	}
	        maxD = 1;
	    	}
	    return null;
		}
	/**Estimate the cost for the motion between two points, without any knowledge of the actual path between*/
	private int heuristic_cost_estimate(Point start, Point goal)
		{
		return Calc.approxIntegerDistance(32*(goal.x-start.x), 32*(goal.y-start.y));
		}
	/**Reconstruct a path through the Grid from the A* result.*/
	public void reconstruct_path(HashMap<Point, Point> came_from, Point current_node, ArrayList<Point> path)
	    {
	    if (came_from.containsKey(current_node))
	        reconstruct_path(came_from, came_from.get(current_node), path);
	    path.add(current_node);
		}
	/**
	 * Use the A* algorithm on the Grid to find a path from the start to the goal in world space.
	 * @param start The start position in world space.
	 * @param goal The goal position in world space.
	 * @param diagonals Whether to allow diagonal motion.
	 * @param maxUnstuckDistance The maximum amount of move on the first step, 
	 * in grid coordinates, in order to break free from being stuck in a wall.
	 * @param blockedRadius The amount of extra space that must be free on both sides, 
	 * in grid coordinates, for a position to count as open.
	 * @return A set of points in world space, 
	 * where the first point is the start location and the last point is the goal location.
	 * Special cases:<br/>
	 * The goal location if the start is on the same grid space as the goal.<br/>
	 * Null if no path could be found.
	 */
	public V3F[] findPath(V3F start, V3F goal, boolean diagonals, int maxUnstuckDistance, int blockedRadius)
		{
		Point[] path = findPath(
				new Point(getGridX(start.x()), getGridY(start.y())), 
				new Point(getGridX(goal.x()), getGridY(goal.y())), diagonals, maxUnstuckDistance, blockedRadius);
		if(path == null)
			return null;
		if(path.length <= 1)
			return new V3F[]{goal.clone()};
		
		float z = start.z();
		float zIncr = (goal.z()-start.z())/(path.length-1);
		V3F[] r = new V3F[path.length];
		for(int i = 0; i<r.length; i++)
			{
			Point p = path[i];
			r[i] = pos.clone().add((0.5f+p.x)*xScale, (0.5f+p.y)*yScale, 0);
			r[i].z(z);
			z += zIncr;
			}
		return r;
		}
	
	/**Render the grid in world space. This method is inefficient and only useful for debugging.*/
	public void render(GL2 gl)
		{
		gl.glPushMatrix();
		gl.glTranslated(pos.x()+xScale/2, pos.y()+yScale/2, pos.z());
		gl.glScalef(xScale, yScale, 1);
		for(int y = 0; y<grid.length; y++)
			{
			gl.glPushMatrix();
			for(int x = 0; x<grid[0].length; x++)
				{
				boolean s = grid[y][x];
				
				gl.glColor4f(s == BLOCKED?1:0, s == OPEN?1:0, 0, 0.2f);
				gl.glPushMatrix();
				gl.glScalef(0.5f, 0.5f, 1);
				gl.glTranslatef(0, 0, 0);
				gl.glBegin(GL2.GL_TRIANGLE_STRIP);
				gl.glVertex2f(-1, -1);
				gl.glVertex2f(-1, 1);
				gl.glVertex2f(1, -1);
				gl.glVertex2f(1, 1);
				gl.glEnd();
				gl.glPopMatrix();
				gl.glTranslatef(1, 0, 0);
				}
			gl.glPopMatrix();
			gl.glTranslatef(0,1,0);
			}
		gl.glPopMatrix();
		}
	/**
	 * Returns the number of blocks to move in the indicated direction in order to reach the edge of the grid.
	 * @param gridPoint The point on the grid.
	 * @param dir4 The counterclockwise cardinal direction, 0-3, where 0 is east.
	 * @return The number of points it is possible to travel 
	 * before reaching the farthest point on the grid in the given direction.
	 */
	public int getGridDistanceToEnd(Point gridPoint, int dir4)
		{
		if(dir4 == 0)
			{
			return getGridWidth()-gridPoint.x-1;
			}
		else if(dir4 == 1)
			{
			return gridPoint.y;
			}
		else if(dir4 == 2)
			{
			return gridPoint.x;
			}
		else if(dir4 == 3)
			{
			return getGridHeight()-gridPoint.y-1;
			}
		System.err.println("Warning: bogus direction sent to getGridDistanceToEnd: "+dir4);
		new Throwable().printStackTrace();
		return 0;
		}
	/**Returns the width of the grid in grid space.*/
	public int getGridWidth()
		{
		return grid[0].length;
		}
	/**Returns the height of the grid in grid space.*/
	public int getGridHeight()
		{
		return grid.length;
		}
	
	/**Converts a vector speed into a counterclockwise integer direction from 0 through 3, where 0 is east.*/
	public static int getDir4(V3F vectorSpeed)
		{
		return getDir4(Calc.dir(0, 0, vectorSpeed.x(), vectorSpeed.y()));
		}
	/**Converts a radian angle into a counterclockwise integer direction from 0 through 3, where 0 is east.*/
	public static int getDir4(double angle)
		{
		return Calc.cyclic((int)Math.round(4.0*angle/(2*Math.PI)), 4);
		}
	/**Clear the grid to make it completely open.*/
	public void clear() 
		{
		for(int y = 0; y<grid.length; y++)
			{
			for(int x = 0; x<grid[0].length; x++) 
				{
				grid[y][x] = OPEN;
				}
			}
		}
	}
